#include "Aria.h"

#define NUM_SONAR 8
#define MAX_VELOCITY 1200

enum Directions
{
	NO_DIRECTION = 0,
	LEFT = 1,
	RIGHT = 2,
};

int GetMinimum(int* inputArray, int arrayLength)
{
	int minimum=inputArray[0];
	for(int i=1;i<arrayLength;i++)
	{
		if(inputArray[i]<minimum)
		{
			minimum = inputArray[i];
		}
	}
	return minimum;
}

class Robot
{
public:
	ArRobot robot;
	ArArgumentParser parser;
	ArRobotConnector robotConnector;
	
	Robot(int &argc, char ** &argv) : parser(&argc, argv), robotConnector(&parser,&robot)
	{
		
	}	

	void Init()
	{
		Aria::init();
		parser.loadDefaultArguments();

		if(!robotConnector.connectRobot())
		{
			ArLog::log(ArLog::Terse, "simpleConnect: Could not connect to the robot.");
			if(parser.checkHelpAndWarnUnparsed())
			{
				// -help not given
				Aria::logOptions();
				Aria::exit(1);
			}
		}
		if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
		{
			Aria::logOptions();
			Aria::exit(1);
		}

		robot.runAsync(true);
	
		robot.enableMotors();
		robot.enableSonar();

		robot.lock();
		robot.setTransVelMax(1000); // absMax 1000 
		robot.setTransDecel(500);  // absMax 2000 & 1500 found to be improper for safe use
		robot.setTransAccel(750);  // absMax 2000
		robot.setRotVelMax(120);    // absMax 360
		robot.unlock();

		ArUtil::sleep(5000);
	}

	void UnInit()
	{
		ArLog::log(ArLog::Normal, "simpleConnect: Ending robot thread...");
		robot.stopRunning();		

		// wait for the thread to stop
		robot.waitForRunExit();
		ArUtil::sleep(500);
	
		// exit
		ArLog::log(ArLog::Normal, "simpleConnect: Exiting.");
		Aria::exit(0);	
	}

	int GetClosestSonarReading()
	{		
		int i; //Counter for looping		
		ArSensorReading* sonarReading; //To hold each reading

		//Example sonar readings ...
		robot.lock();
		int sonarReadings[NUM_SONAR];
		int minimum;
		for (i = 0; i < NUM_SONAR; i++) //Loop through sonar
		{
			sonarReading = robot.getSonarReading(i);
			sonarReadings[i] = sonarReading->getRange();
			// Get each sonar reading
			// cout << "Sonar reading " << i << " = " << sonarReading->getRange() << " Angle " << i << " = " << sonarReading->getSensorTh() << "\n";
		}
	
		minimum = GetMinimum(sonarReadings, NUM_SONAR);	
		robot.unlock();

		return minimum;
	}
	void SetVel(int velocity)
	{
		robot.lock();
		robot.setVel(velocity);
		robot.unlock();
	}
	void SetRotVel(int rotvelocity)
	{
		robot.lock();
		robot.setRotVel(rotvelocity);
		robot.unlock();	
	}
	void TurnSync(Directions direction)
	{
		int rotVel = direction == Directions::LEFT ? -100 : 100;

		robot.lock();	
		robot.setRotVel(rotVel);
		double firstTh = robot.getTh();
		robot.unlock();
		
		ArUtil::sleep(3650 * 2 - 200);

		robot.lock();
		robot.setRotVel(0);
		robot.unlock();
	}
	bool IsStopNecessary()
	{	
		bool rVal = false;
		int i; //Counter for looping		
		ArSensorReading* sonarReading; //To hold each reading

		//Example sonar readings ...
		robot.lock();
		int sonarReadings[NUM_SONAR];
		int minimum;
		for (i = 0; i < NUM_SONAR; i++) //Loop through sonar
		{
			sonarReading = robot.getSonarReading(i);
			sonarReadings[i] = sonarReading->getRange();
			// Get each sonar reading
			// cout << "Sonar reading " << i << " = " << sonarReading->getRange() << " Angle " << i << " = " << sonarReading->getSensorTh() << "\n";
		}		
		
		for(int i=0; i<NUM_SONAR/2; i++)
		{
			int threshold = (i+1) * 100 + 50;
			if(sonarReadings[i] < threshold || sonarReadings[7-i] <threshold)
			{
				rVal = true;	
			}
		}
		robot.unlock();

		return rVal;
	}
	double GetCurrentVel()
	{
		double rVal = 0;
		robot.lock();
		rVal = robot.getVel();
		robot.unlock();
		return rVal;
	}

};